#pragma once

#include <random>
#include <array>
#include <tuple>

#include <CGAL/memory.h>
#include <CGAL/Iterator_range.h>
#include <CGAL/min_quadrilateral_2.h>
#include <CGAL/HalfedgeDS_vector.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
//#include <CGAL/Surface_mesh.h>
//#include <CGAL/polygon_mesh_processing.h>
#include <CGAL/bounding_box.h>
//include <CGAL/linear_least_squares_fitting_3.h>
#include <PointCloud.h>
#include <Plane.h>
//#include <math/common.hpp>

using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
//using FT = typename Kernel::FT;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
//using Polygon_mesh = CGAL::Surface_mesh<Point_3>;
using Plane_3 = typename Kernel::Plane_3;
//using Face_index = typename Polygon_mesh::Face_index;
//using FaceIndices = std::vector<std::size_t>;
using Rectangle = std::array<double, 12>;
//using vertex_descriptor = Polygon_mesh::Vertex_index;
//using face_descriptor = Polygon_mesh::Face_index;
//using size_type = Polygon_mesh::size_type;
using Points = std::vector<Point_3>;
using Planes = std::vector<Plane_3>;

class CGAL_Utils
{
public:

    static Points getVertices(const Points& points, const Plane_3 plane, float min_area = 1.0)
    {
        // 从这里开始，是需要的部分
        Kernel::Iso_cuboid_3 bbox = CGAL::bounding_box(points.cbegin(), points.cend());
        Point_3 centroid = CGAL::midpoint(bbox.min(), bbox.max());

        constexpr double eps{ 1E-2 };

        if (bbox.is_degenerate())
        {
            std::cout << "bounding box 发生退化" << std::endl;
            exit(-1);
            
            auto mid_point{ CGAL::midpoint(bbox.min(), bbox.max()) };
            double x_size{ bbox.xmax() - bbox.xmin() }, y_size{ bbox.ymax() - bbox.ymin() }, z_size{ bbox.zmax() - bbox.zmin() };

            // Too small size of bbox. Intersections with point my be invalid
            // if (boost::math::epsilon_difference(CGAL::to_double(bbox.xmax()), CGAL::to_double(bbox.xmin())) < eps)
            // {
            //     x_size = 2 * eps;
            // }

            // if (boost::math::epsilon_difference(CGAL::to_double(bbox.ymax()), CGAL::to_double(bbox.ymin())) < eps)
            // {
            //     y_size = 2 * eps;
            // }

            // if (boost::math::epsilon_difference(CGAL::to_double(bbox.zmin()), CGAL::to_double(bbox.zmax())) < eps)
            // {
            //     z_size = 2 * eps;
            // }

            x_size = 2 * eps;
            y_size = 2 * eps;
            z_size = 2 * eps;

            const Vector_3 shift_vector{ x_size / 2, y_size / 2, z_size / 2 };

            bbox = Kernel::Iso_cuboid_3{ mid_point - shift_vector, mid_point + shift_vector };
        }

        // plane 和 bounding box 相交
        auto interesct_points{plane_aabb_intersection_points(plane, bbox)};

        if (interesct_points.size() >= 3)
        {
            if (interesct_points.size() != 4)
                interesct_points = polygon_to_rectangle_on_plane(interesct_points, plane);
            

            // 矩形的长宽
            double length_dir1 = CGAL::sqrt((interesct_points.at(1) - interesct_points.at(0)).squared_length());
            double length_dir2 = CGAL::sqrt((interesct_points.at(3) - interesct_points.at(0)).squared_length());

            if (length_dir1 * length_dir2 >= min_area)
            {
                //interesct_points = expandIntersect(interesct_points);

                auto p_min{interesct_points.at(0)}, p_max{interesct_points.at(2)};

                auto rectangle_center{CGAL::midpoint(p_min, p_max)};

                Vector_3 shifting{centroid - rectangle_center};

                for (size_t i = 0, j = 0; i < interesct_points.size(); i++, j += 3)
                    interesct_points.at(i) = interesct_points.at(i) + shifting;
                
                return interesct_points;
            }
            else{
                std::cout << u8"The rect area is smaller than min_area." << std::endl;
                return points;
            }
        }
        else if (interesct_points.size() > 0 && interesct_points.size() < 3)
        {
            std::cout << u8"Number of intersections between plane and bbox is incorrect. Number of intersections:" << interesct_points.size() << std::endl;
            return points;
        }
    }

    static Points getVertices(const Plane& p, float min_area = 1.0)
    {
        // 数据转换部分
        //----------------------------------------
        Points points;
        for(auto &pt : p.getPointCloud()->points)
            points.push_back(Point_3(pt.x, pt.y, pt.z));

        Plane_3 plane(p.getA(), p.getB(), p.getC(), -p.getD());
        //----------------------------------------

        return getVertices(points, plane, min_area);
    }

    static Points getExpandVertices(Points old_points, std::pair<int,int> edge)
    {
        return getExpandVertices(old_points, edge.first, edge.second);
    }
    
    static Points getExpandVertices(Points old_points, int min_idx, int min_idx2)
    {
        Points new_points = old_points;
        int b = getOther(min_idx, min_idx2);
        new_points[min_idx] = shiftPoint(old_points[b], old_points[min_idx]);

        int b2 = getOther(min_idx2, min_idx);
        new_points[min_idx2] = shiftPoint(old_points[b2], old_points[min_idx2]);

        return new_points;
    }

    static float getDistance(Point_3 p1, Point_3 p2)
    {
        return get_distance( Vector3d(p1.x(), p1.y(), p1.z()),
                             Vector3d(p2.x(), p2.y(), p2.z()) );
    }

private:
    // Find intersections between AABB and plane.
    static std::vector<Point_3> plane_aabb_intersection_points(const Kernel::Plane_3 &plane, const Kernel::Iso_cuboid_3 &bbox)
    {
        std::vector<Point_3> points;

        // See https://doc.cgal.org/latest/Kernel_23/classCGAL_1_1Iso__cuboid__3.html#a48f499351f3ef6d80751115372489729
        std::array<std::tuple<int, int>, 12> end_points{
            std::make_tuple(0, 1),
            std::make_tuple(3, 2),
            std::make_tuple(5, 6),
            std::make_tuple(4, 7),
            std::make_tuple(0, 3),
            std::make_tuple(1, 2),
            std::make_tuple(5, 4),
            std::make_tuple(6, 7),
            std::make_tuple(0, 5),
            std::make_tuple(1, 6),
            std::make_tuple(2, 7),
            std::make_tuple(3, 4)};

        int i{};

        for (const auto &end_point : end_points)
        {
            Kernel::Segment_3 segment{bbox.vertex(std::get<0>(end_point)), bbox.vertex(std::get<1>(end_point))};

            auto intersection = CGAL::intersection(segment, plane);

            if (intersection)
            {
                if (const Kernel::Point_3 *p = boost::get<Kernel::Point_3>(&*intersection))
                {
                    points.emplace_back(*p);
                }
                // Include ends
                else if (i < 4)
                {
                    if (const Kernel::Segment_3 *seg = boost::get<Kernel::Segment_3>(&*intersection))
                    {
                        points.emplace_back(seg->start());
                        points.emplace_back(seg->end());
                    }
                }
            }

            i++;
        }

        if (!points.empty())
        {
            const Vector_3 plane_normal = plane.orthogonal_vector();
            const Point_3 origin = *points.cbegin();

            // Sort in counter clockwise order
            std::sort(points.begin(), points.end(), [&origin, &plane_normal](const Point_3 &lhs, const Point_3 &rhs) -> bool {
                auto v{CGAL::cross_product(lhs - origin, rhs - origin)};
                //return CGAL::scalar_product(v, plane_normal) < 0;
                return v * plane_normal < 0;
            });
        }

        return points;
    }

    static std::vector<Point_3> polygon_to_rectangle_on_plane(const std::vector<Point_3> &points, const Kernel::Plane_3 &plane)
    {
        using Point_2 = Kernel::Point_2;

        assert(points.size() >= 3);

        // Choosing new basis on the linear subspace. The linear subspace is plane.
        Point_3 new_origin{points.at(0)};

        Vector_3 new_basis_1;

        for (size_t i{1}; i < points.size(); i++)
        {
            new_basis_1 = {points.at(i) - new_origin};

            if (!CGAL::is_zero(new_basis_1.squared_length()))
            {
                break;
            }
        }

        Vector_3 new_basis_3{plane.orthogonal_vector()};
        Vector_3 new_basis_2{CGAL::cross_product(new_basis_1, new_basis_3)};
        Vector_3 origin_shifting{new_origin - CGAL::ORIGIN};

        // Matrix of a transformation with respect to a basis. From new coordinate system to origin.
        Kernel::Aff_transformation_3 transformation{
            new_basis_1.x(), new_basis_2.x(), new_basis_3.x(), 0,
            new_basis_1.y(), new_basis_2.y(), new_basis_3.y(), 0,
            new_basis_1.z(), new_basis_2.z(), new_basis_3.z(), 0};

        Kernel::Aff_transformation_3 inveres_transformation{transformation.inverse()};

        // Projections on plane. All points on plane have third coordinate is equal to zero.
        std::vector<Point_2> new_projection_points(points.size(), CGAL::ORIGIN);

        std::transform(points.cbegin(), points.cend(), new_projection_points.begin(), [&inveres_transformation](const Point_3 &point) {
            auto projection{point.transform(inveres_transformation)};
            return Point_2{projection.x(), projection.y()};
        });

        const int num_rectangle_vertices{4};

        std::vector<Point_2> rectangle_projection{num_rectangle_vertices, CGAL::ORIGIN};

        CGAL::min_rectangle_2(new_projection_points.cbegin(), new_projection_points.cend(), rectangle_projection.begin());

        std::vector<Point_3> rectangle{num_rectangle_vertices, CGAL::ORIGIN};

        std::transform(rectangle_projection.cbegin(), rectangle_projection.cend(), rectangle.begin(), [&transformation](const Point_2 &point) {
            return Point_3{point.x(), point.y(), 0}.transform(transformation);
        });

        return rectangle;
    }

    static int getOther(int idx1, int idx2)
    {
        switch(idx1)
        {
            case 0:
                return (idx2 == 1) ? 3:1;
            case 1:
                return (idx2 == 0) ? 2:0;
            case 2:
                return (idx2 == 1) ? 3:1;
            case 3:
                return (idx2 == 0) ? 2:0;
        };
    }

    static Point_3 shiftPoint(Point_3 begin, Point_3 end, float scale = 1.20)
    {
        Vector_3 vec(begin, end);
        vec = vec*scale;
        return begin+vec;
    }

    // static Point_3 expandPoint(Point_3 p, Point_3 b1, Point_3 b2)
    // {
    //     Point_3 p_1 = shiftPoint(b1, p);
    //     Point_3 p_2 = shiftPoint(b2, p);
    //     return CGAL::midpoint(p_1, p_2);
    // }

    // static Points expandIntersect(Points old_points)
    // {
    //     Points new_points;
    //     new_points.push_back(expandPoint(old_points.at(0), old_points.at(1), old_points.at(3)));
    //     new_points.push_back(expandPoint(old_points.at(1), old_points.at(0), old_points.at(2)));
    //     new_points.push_back(expandPoint(old_points.at(2), old_points.at(1), old_points.at(3)));
    //     new_points.push_back(expandPoint(old_points.at(3), old_points.at(0), old_points.at(2)));
    //     return new_points;
    // }

    // static Rectangle getRectExpandBottom(Points interesct_points)
    // {
    //     return getRect(expandIntersect(interesct_points));
    // }

    // static Rectangle getRect(const Points& interesct_points)
    // {
    //     Rectangle rect;
    //     for (size_t i = 0, j = 0; i < interesct_points.size(); i++, j += 3)
    //     {
    //         rect[j] = CGAL::to_double(interesct_points.at(i).x());
    //         rect[j + 1] = CGAL::to_double(interesct_points.at(i).y());
    //         rect[j + 2] = CGAL::to_double(interesct_points.at(i).z());
    //     }
    //     return rect;
    // }

    // Rectangle getRect(Plane p)
    // {
    //     return getRect(getRectPoints(p));
    // }
    
    // Points getRectPoints(Plane p)
    // {
    //     Points pts = Point3Ds_to_Points(p.getPoints());
    //     return getIntersectPoints(pts, Plane_to_Plane_3(p));
    // }

    // 墙面有多个
    // std::vector<Rectangle> getRectangles(std::vector<Points> &points, Planes &planes, Points &centroids, float min_area = 1.0)
    // {
    //     std::vector<Rectangle> rectangles;
    //     for (int i = 0; i < points.size(); ++i)
    //         rectangles.push_back(getRectangle(points[i], planes[i], centroids[i], min_area));
    //     return rectangles;
    // }

    // static Points Rectangle_to_Points(const Rectangle& rect)
    // {
    //     Points points;
    //     for(int i=0; i<12; i+=3)
    //         points.push_back(Point_3(rect[i], rect[i+1], rect[i+2]));

    //     return points;
    // }

    // static Vector3d Point_3_to_Vector3d(Point_3 p)
    // {
    //     return Vector3d(p.x(), p.y(), p.z());
    // }

    // Point_3 Vector3d_to_Point_3(Eigen::Vector3d v)
    // {
    //     return Point_3(v.x(), v.y(), v.z());
    // }


};